 /**************************************************************
 *  Filename:    base_controller_udp.cpp
 *  Copyright:   西安爱极客，Xi'an Aijike Network Technology Co., Ltd
 *  Description: 1.从底盘采集Odom，IMU数据，并发布话题
 *               2.订阅cmd_Vel,并发送至底盘
 *               3.通信方式：UDP透传
 *               4.和底盘之间的通信协议：ModbusRtu over UDP master
 *  @author:     孙毅明
 *  @version     2022.10.06 
 **************************************************************/
/******************************************************************
基于UDPSocket和底盘通信
功能如下：
1.通过订阅/cmd_vel主题，将ROS速度信息通过自定义格式，发送给底盘
2.接收小车发来的里程信息，转换并发布到odm节点

通信说明：
1.写入
（1）内容：线速度和角速度
（2）格式：modbus协议，寄存器地址454-465
2.读取
（1）内容：小车x,y坐标，方向角，线速度，角速度，单位依次为：m,m,rad,m/s,rad/s
（2）格式：modbus协议，寄存器地址466-475
*******************************************************************/
#include "ros/ros.h" //ros需要的头文件
#include <geometry_msgs/Twist.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include "sensor_msgs/Range.h"
//以下为NET通讯需要的头文件
#include <stdio.h>
#include <string.h>
#include <iostream>
#include <stdlib.h>
#include <arpa/inet.h> 
#include <sys/socket.h>

#include "Modbus_master.h"
/****************************************************************************/
using std::cerr;
using std::cout;
using std::endl;
using std::exception;
using std::string;
using std::vector;

/******************************UDP***********************************************/
#define BUF_SIZE 1024
#define ADDR "192.168.0.13" //小车的UDP server IP
#define server_PORT 502      //小车的UDP server PORT
#define local_PORT 502
/*****************************************************************************/

/****************************************************/
unsigned char speed_data[100] = {0}; //要发给的数据
string rec_buffer;                   //数据接收变量
string result;

/////////////////////////////////////////////
// modbus
/////////////////////////////////////////


//发送给下位机的左右轮速度，里程计的坐标和方向
union floatData //共用体union的作用为实现char数组和float之间的转换
{
    float data;
    unsigned char buf[4];
} linear_x, linear_y, linear_z, orientation_x, orientation_y, orientation_z, position_x, position_y, oriention, vel_linear, vel_angular, imu,ut_vel;

/************************************************************/
void callback(const geometry_msgs::Twist &cmd_input) //订阅/cmd_vel主题回调函数
{
    // 分别将线速度和角速度映射到联合体
    linear_x.data = cmd_input.linear.x;
    linear_y.data = cmd_input.linear.y;
    linear_z.data = cmd_input.linear.z;

    orientation_x.data = cmd_input.angular.x;
    orientation_y.data = cmd_input.angular.y;
    orientation_z.data = cmd_input.angular.z;
    //将速度写入预发送数组
    for (int i = 0; i < 4; i++)
    {
        speed_data[i] = linear_x.buf[i];
        speed_data[i + 4] = linear_y.buf[i];
        speed_data[i + 8] = linear_z.buf[i];

        speed_data[i + 12] = orientation_x.buf[i];
        speed_data[i + 16] = orientation_y.buf[i];
        speed_data[i + 20] = orientation_z.buf[i];
    }
    // ROS_INFO("linear_x=%0.4f linear_y=%0.4f linear_z=%0.4f\n",linear_x.data,linear_y.data,linear_z.data);
    // ROS_INFO("orientation_x=%0.4f orientation_y=%0.4f orientation_z=%0.4f\n",orientation_x.data,orientation_y.data,orientation_z.data);
}

int main(int argc, char **argv)
{
    // 这是语言，使打印支持中文
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "base_controller");                                     //初始化节点

    ROS_WARN("爱极客地盘控制节点，自动发布odom，imu，订阅cmd_vel");
    // 获取参数
    string StrParam; 
    ros::param::get("/base_control/ip",StrParam); 
    ROS_INFO("The ip is:%s",StrParam.c_str());

    /*////////////////////////odom/////////////////////////////*/
    ros::NodeHandle contro_n;                                                     //定义节点进程句柄
    ros::Subscriber sub = contro_n.subscribe("/turtle1/cmd_vel", 20, callback);   //订阅/cmd_vel主题
    ros::Publisher odom_pub = contro_n.advertise<nav_msgs::Odometry>("odom", 20); //定义要发布/odom主题
    geometry_msgs::TransformStamped odom_trans; //创建一个tf发布需要使用的TransformStamped类型消息
    nav_msgs::Odometry odom;                    //定义里程计对象
    geometry_msgs::Quaternion odom_quat;        //四元数变量
    //定义covariance矩阵，作用为解决weizhi和速度的不同测量的不确定性
    float covariance[36] = {1e-3, 0, 0, 0, 0, 0, // covariance on gps_x
                            0, 1e-3, 0, 0, 0, 0, // covariance on gps_y
                            0, 0, 1e6, 0, 0, 0,  // covariance on gps_z
                            0, 0, 0, 1e6, 0, 0,  // large covariance on rot x
                            0, 0, 0, 0, 1e6, 0,  // large covariance on rot y
                            0, 0, 0, 0, 0, 1e3}; // large covariance on rot z
    //载入covariance矩阵
    for (int i = 0; i < 36; i++)
    {
        odom.pose.covariance[i] = covariance[i];
    }

    /*////////////////////////imu//////////////////////////////*/
    ros::init(argc, argv, "igk_imu");//初始化节点
    ros::NodeHandle imu_n;  //定义节点进程句柄
    ros::Publisher IMU_pub = imu_n.advertise<sensor_msgs::Imu>("imu_data", 20);  //定义要发布/imu主题
    sensor_msgs::Imu imu_data;

    /***********************************************UDP-NET*****************************************/
    int sock;
    struct sockaddr_in serv_addr;
    memset(&serv_addr, 0, sizeof(serv_addr));
    unsigned int addr_len = sizeof(serv_addr);

    serv_addr.sin_family = AF_INET;
    serv_addr.sin_port = htons(server_PORT);     /// server的监听端口
    serv_addr.sin_addr.s_addr = inet_addr(ADDR); // server IP

    struct sockaddr_in serverGet;
    memset(&serverGet, 0, sizeof(serverGet));

    serverGet.sin_family = AF_INET;
    serverGet.sin_port = htons(local_PORT); // local port
    serverGet.sin_addr.s_addr = INADDR_ANY;

    sock = socket(AF_INET, SOCK_DGRAM, 0);
    if (sock < 0)
    {
        ROS_INFO("failed socket");
        return -1;
    }

    bind(sock, (struct sockaddr *)&serverGet, sizeof serv_addr);

    // 设置超时时间，防止UDP 没有接收到数据是一直卡在接受函数
    timeval tv = {0, 100*1000};//{秒，微妙} 100毫秒
    
    int32_t b = setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (char*)&tv, sizeof(timeval));
    if (b)
    {
        ROS_INFO("setsockopt failed");
        return 0;
    }
    /*************************************************************************************************/

    //指定循环的频率
    ros::Rate loop_rate(50);
    while (ros::ok())
    {
        //发送角速度跟线速度给下位机
        // 计算要发送的数组
        uint8_t len = WriteMultipleHoldRegister(454, (uint16_t *)speed_data, 12);
        int tx_len = sendto(sock, ModbusMasterSendBuf, len, 0, (struct sockaddr *)&serv_addr, addr_len);
        // if (tx_len != len)
        // {
        //     ROS_INFO("TX erro\n");
        // }
        // else
        // {
        //     ROS_INFO("TX OK\n");
        // }

        //接收UDP的数据并缓存
        char get_msg[BUF_SIZE];
        int recvLen = recvfrom(sock, get_msg, 1024, 0, NULL, NULL);
        if (recvLen > 0)
        {
            // 打印反馈数据
            // for (int i = 0; i < recvLen; i++)
            // {
            //     ROS_INFO("%3d： 0x%02X ", i,(uint8_t)get_msg[i]);
            // }

            // ROS_INFO("len:%d\n", recvLen);
        }

        // 读数据 计算要发送的数组
        len = ReadHoldRegister(466, 12);
        tx_len = sendto(sock, ModbusMasterSendBuf, len, 0, (struct sockaddr *)&serv_addr, addr_len);
        // 接收
        recvLen = recvfrom(sock, get_msg, 1024, 0, NULL, NULL);
        if (recvLen == 29)
        {
            for (int i = 0; i < recvLen; i++)
            {
                ROS_INFO("%d:0x%02X ",i,(uint8_t)get_msg[i]);
            }
            ROS_INFO("len:%d\n", recvLen);

            //映射到对象
            int16_t start = 3;
            uint8_t buff[4] = {1,0,3,2};
            for(int i=0;i<4;i++)    
            {
                position_x.buf[buff[i]] = get_msg[start+i];
                position_y.buf[buff[i]] = get_msg[start+i+4];
                oriention.buf[buff[i]] = get_msg[start+i+8];

                vel_linear.buf[buff[i]] = get_msg[start+i+12];
                vel_angular.buf[buff[i]] = get_msg[start+i+16];
                imu.buf[buff[i]] = get_msg[start+i+20];

                ROS_INFO("position_x=%0.4f position_y=%0.4f oriention=%0.4f\n",position_x.data,position_y.data,oriention.data);
                ROS_INFO("vel_linear=%0.4f vel_angular=%0.4f imu=%0.4f\n",vel_linear.data,vel_angular.data,imu.data);
            } 

            ////////////////////////////////////////////////////////////////////
            //                            发布里程计数据                        //
            ////////////////////////////////////////////////////////////////////
            //里程计的偏航角需要转换成四元数才能发布                     
            odom_quat = tf::createQuaternionMsgFromYaw(oriention.data);//将偏航角转换成四元数   
            //载入里程计时间戳
            odom.header.stamp = ros::Time::now(); 
            //里程计的父子坐标系
            odom.header.frame_id = "base_link";
            odom.child_frame_id = "odom";       
            //里程计位置数据：x,y,z,方向
            odom.pose.pose.position.x = position_x.data;     
            odom.pose.pose.position.y = position_y.data;
            odom.pose.pose.position.z = 0.0;
            odom.pose.pose.orientation = odom_quat;       
            //载入线速度和角速度
            odom.twist.twist.linear.x = vel_linear.data;
            odom.twist.twist.angular.z = vel_angular.data;    
            //发布里程计
            odom_pub.publish(odom);    
                     
        }



        ////////////////////////////////////////////////////////////////////
        //                           处理IMU数据                           //
        ////////////////////////////////////////////////////////////////////
        imu_data.header.stamp = ros::Time::now();
        imu_data.header.frame_id = "base_link";

        /****************RPY欧拉角转四元数*****************/ 
        //0-360转正负180度
        if(imu.data>180)
            imu.data = imu.data-360;
        //只通过y即绕z的旋转角度计算四元数，用于平面小车。返回四元数
        imu_data.orientation = tf::createQuaternionMsgFromYaw(imu.data*M_PI/180.0f);//这里将度转为了弧度
        IMU_pub.publish(imu_data);
        //程序周期性调用
        ros::spinOnce();   // callback函数必须处理所有问题时，才可以用到
        loop_rate.sleep(); //周期休眠
    }

    close(sock);
    return 0;
}
